#include <TL/robotManipulationDomain.h>


RobotManipulationSimulator sim;


void initSimulator(const char* configurationFile, bool takeMovie) {
  sim.shutdownAll();
  sim.loadConfiguration(configurationFile);
#ifdef MT_FREEGLUT
  orsDrawProxies = false;
  orsDrawJoints = false;
#endif
  sim.startOde();
  if (takeMovie)
    sim.startRevel();
  sim.startSwift();
  sim.simulate(50);
}



void test(int argn,char** argv) {
	
  uint randSeed = 12345;
  rnd.seed(randSeed);
  uint num_actions = 50;

  MT::String sim_file("../../configurations/situation.ors");
  
  initSimulator(sim_file, false);
  sim.simulate(50);

  // -------------------------------------
  //  SOME SIMULATOR INFORMATION
  // -------------------------------------
  cout<<"GENERAL SIMULATOR INFORMATION:"<<endl;
  uintA objects;
  sim.getObjects(objects);
  cout<<"Objects: "<<objects<<endl;

  uint id_table = sim.getTableID();
  cout<<"Table: "<<id_table<<endl;
  
  uintA blocks;
  sim.getBlocks(blocks);
  cout<<"Blocks: "<<blocks<<endl;
	
  uintA balls;
  sim.getBalls(balls);
  cout<<"Balls: "<<balls<<endl;
  
  cout<<"Sizes:"<<endl;
  uint i;
  FOR1D(objects, i) {
    double* size = sim.getSize(objects(i));
    cout<<objects(i)<<":  "<<size[0]<<endl;
  }

  // -------------------------------------
  //  SOME RANDOM ACTIONS
  // -------------------------------------
  uint target_object;
  uint k;
  for (k=0; k<num_actions; k++) {
    // STATE INFO
    cout<<endl<<"STATE:"<<endl;
    FOR1D(objects, i) {
      cout<<objects(i)<<": ";
      uintA above_objects;
      sim.getObjectsAbove(above_objects, objects(i));
      cout<<"above_objects="<<above_objects<<"  ";
      if (sim.isUpright(objects(i))) {
	cout<<"upright  ";
      }
      if (sim.onGround(objects(i))) {
	cout<<"onground  ";
      }
      if (sim.isClear(objects(i))) {
	cout<<"clear  ";
      }
      cout<<endl;
    }
    uint inhand_object = sim.getInhand();
    if (sim.getInhand() == UINT_MAX)
      cout<<"Inhand: -"<<endl;
    else
      cout<<"Inhand: "<<inhand_object<<endl;
    cout<<"Please press button to continue."<<endl;
    sim.watch();
    // ACTION
    cout<<endl<<"ACTION:"<<endl;
    // random object
    target_object = objects(rnd.num(objects.N));
    if (k%2 == 0) { // grab
      cout<<"Grabbing "<<target_object<<"."<<endl;
      sim.grab(target_object);
    }
    else { // put on
      cout<<"Putting on "<<target_object<<"."<<endl;
      sim.dropObjectAbove(target_object);
    }
    sim.relaxPosition();
    sim.simulate(50);
  }
  
  cout<<"Please press button to continue."<<endl;
  sim.watch();
  
  sim.shutdownAll();
}


int main(int argc, char** argv){
  test(argc, argv);
  return 0;
}
